/*
Library for Arduino AdaFruit MotorShield
Author: Jeff Szcinski II (jeffski)
*/

#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include <MotorShield.h>

/**
constructor:
takes care of assigning motors for you 

@param m1 integer value which corresponds to the input on the motorshield M1
@param m2 integer value which corresponds to the input on the motorshield M2
@param m3 integer value which corresponds to the input on the motorshield M3
@param m4 integer value which corresponds to the input on the motorshield M4
*/
MotorShield::MotorShield(int m1, int m2, int m3, int m4){
	//turn debug statements off by default
	debug = false;
	// Creates the motor shield object with the default I2C address
	AFMS = Adafruit_MotorShield();
	
	// Select which 'port' M1, M2, M3 or M4
	myMotor1 = AFMS.getMotor(m1);  // M1 Front L
	myMotor2 = AFMS.getMotor(m2);  // M2 Front R
	myMotor3 = AFMS.getMotor(m3);  // M3 Rear L
	myMotor4 = AFMS.getMotor(m4);  // M4 Rear R
}

/**
should be called during setup() in arduino
*/
void MotorShield::begin(){
	AFMS.begin();
}

/**
moves the robot forward at a specific speed

@param motorSpeed the speed for the motors. can be 0-255
@see MotorShield::stop()
*/
void MotorShield::moveForward(int motorSpeed){
	if(debug){
		Serial.print("MotorShield- going forward: ");
		Serial.println(motorSpeed);
	}
	
	myMotor1->run(FORWARD);
	myMotor2->run(BACKWARD);
	myMotor3->run(FORWARD);
	myMotor4->run(FORWARD);

	set_speed(motorSpeed);
}

/**
moves the robot backwards at a specific speed

@param motorSpeed the speed for the motors. can be 0-255
@see MotorShield::stop()
*/
void MotorShield::moveBackward(int motorSpeed){
	if(debug){
		Serial.print("MotorShield- going backwards: ");
		Serial.println(motorSpeed);
	}
	
	myMotor1->run(BACKWARD);
	myMotor2->run(FORWARD);
	myMotor3->run(BACKWARD);
	myMotor4->run(BACKWARD);

	set_speed(motorSpeed);
}

/**
stop robot
*/
void MotorShield::stop(){
	if(debug)
		Serial.println("MotorShield- STOP");
  
	myMotor1->run(RELEASE);
	myMotor2->run(RELEASE);
	myMotor3->run(RELEASE);
	myMotor4->run(RELEASE);
}

/**
turns robot to the right. uses tank style turning so robot will turn about center axis

@param motorSpeed the speed for the motors. can be 0-255
@see MotorShield::stop()
*/
void MotorShield::turnRight(int motorSpeed){
	if(debug){
		Serial.print("MotorShield- turning right: ");
		Serial.println(motorSpeed);
	}
	
	myMotor1->run(FORWARD);
	myMotor2->run(FORWARD);
	myMotor3->run(FORWARD);
	myMotor4->run(BACKWARD);

	set_speed(motorSpeed);
}
void MotorShield::turnRight_spin(int motorSpeed){
   if(debug){
		Serial.print("MotorShield- turning right: ");
		Serial.println(motorSpeed);
	}
	
	myMotor1->run(FORWARD);
	myMotor2->run(FORWARD);
	myMotor3->run(FORWARD);
	myMotor4->run(BACKWARD);

	set_speed_spin2(motorSpeed);
}

/**
turns robot to the left. uses tank style turning so robot will turn about center axis

@see MotorShield::stop()
*/
void MotorShield::turnLeft(int motorSpeed){
	if(debug){
		Serial.print("MotorShield- turning left: ");
		Serial.println(motorSpeed);
	}
	
	myMotor1->run(BACKWARD);
	myMotor2->run(BACKWARD);
	myMotor3->run(BACKWARD);
	myMotor4->run(FORWARD);

	set_speed(motorSpeed);
}
void MotorShield::turnLeft_spin(int motorSpeed){
   if(debug){
		Serial.print("MotorShield- turning left: ");
		Serial.println(motorSpeed);
	}
	
	myMotor1->run(BACKWARD);
	myMotor2->run(BACKWARD);
	myMotor3->run(BACKWARD);
	myMotor4->run(FORWARD);

	set_speed_spin2(motorSpeed);
}

/**
turns the motors on, sets the speed
the left motors (m1 and m3) are slower to account for the robot
veering slightly when it moves forward in a straight line

@param motorSpeed the speed for the motors. can be 0-255
*/
void MotorShield::set_speed(int motorSpeed){
	
	myMotor1->setSpeed(motorSpeed+30);
	myMotor2->setSpeed(motorSpeed);
	myMotor3->setSpeed(motorSpeed+30);
	myMotor4->setSpeed(motorSpeed);
}
void MotorShield::set_speed_spin1(int motorSpeed){
	myMotor1->setSpeed(motorSpeed);
	myMotor2->setSpeed(motorSpeed);
	myMotor3->setSpeed(motorSpeed);
	myMotor4->setSpeed(motorSpeed);
}
void MotorShield::set_speed_spin2(int motorSpeed){
   myMotor1->setSpeed(motorSpeed+10);
	myMotor2->setSpeed(motorSpeed);
	myMotor3->setSpeed(motorSpeed+10);
	myMotor4->setSpeed(motorSpeed);
}

// -------------------------------------------------------------------------------------
/**
turns the motors on, sets the speed
the right motors (m2 and m4) are slower to make the robot
veer to the right when moving forward

@param motorSpeed the speed for the motors.  can be 0-255
*/
void MotorShield::veerRight(int motorSpeed){
   if(debug){
		Serial.print("MotorShield- veering right: ");
		Serial.println(motorSpeed);
	}
   
   // set motors in forward orientation
   myMotor1->run(FORWARD);
	myMotor2->run(BACKWARD);
	myMotor3->run(FORWARD);
	myMotor4->run(FORWARD);
   
   // speed offsets
   myMotor1->setSpeed(motorSpeed+30);
	myMotor2->setSpeed(motorSpeed-100);
	myMotor3->setSpeed(motorSpeed+30);
	myMotor4->setSpeed(motorSpeed-100);
}

/**
turns the motors on, sets the speed
the left motors (m1 and m3) are slower to make the robot
veer to the left when moving forward

@param motorSpeed the speed for the motors.  can be 0-255
*/
void MotorShield::veerLeft(int motorSpeed){
   if(debug){
		Serial.print("MotorShield- veering left: ");
		Serial.println(motorSpeed);
	}
   
   // set motors in forward orientation
   myMotor1->run(FORWARD);
	myMotor2->run(BACKWARD);
	myMotor3->run(FORWARD);
	myMotor4->run(FORWARD);
   
   // speed offsets
   myMotor1->setSpeed(motorSpeed-70);
	myMotor2->setSpeed(motorSpeed);
	myMotor3->setSpeed(motorSpeed-70);
	myMotor4->setSpeed(motorSpeed);
}
// -------------------------------------------------------------------------------------

/**
test a specific motor

@param mTest the motor which you would like to test
*/
void MotorShield::testMotor(int mTest){
	if(debug){
		Serial.print("MotorShield- testing motor ");
		Serial.println(mTest);
	}
	
	if(mTest == 1){
		myMotor1->run(FORWARD);
		myMotor2->run(RELEASE);
		myMotor3->run(RELEASE);
		myMotor4->run(RELEASE);
		
		myMotor1->setSpeed(128);
	}
	else if(mTest == 2){
		myMotor1->run(RELEASE);
		myMotor2->run(BACKWARD);
		myMotor3->run(RELEASE);
		myMotor4->run(RELEASE);
		
		myMotor2->setSpeed(128);
	}
	else if(mTest == 3){
		myMotor1->run(RELEASE);
		myMotor2->run(RELEASE);
		myMotor3->run(FORWARD);
		myMotor4->run(RELEASE);
		
		myMotor3->setSpeed(128);
	}
	else if(mTest == 4){
		myMotor1->run(RELEASE);
		myMotor2->run(RELEASE);
		myMotor3->run(RELEASE);
		myMotor4->run(FORWARD);
		
		myMotor4->setSpeed(128);
	}
	else{
		Serial.print("MotorShield- INVALID MOTOR ENTRY ");
		Serial.println(mTest);
	}
}


/**
turns on debug statements
*/
void MotorShield::setDebugOn(){
	debug = true;
	Serial.print("debug on");
}

/**
turns off debug statements
*/
void MotorShield::setDebugOff(){
	debug = false;
}